% This file is for analysing the 
% This file illustrates that the result of parallel and single-process data are not the same. However, you should note that the measurement is noised by random number 
% So, consequent simulation are needed.
close all 
path(path,'../libs/publicMatlabLibs/');
SIMU_FOLDER = '../data/exp_noised_8_2/';
SIMU_MEASUREMENT_FOLDER = strcat(SIMU_FOLDER , 'measurement/');
SIMU_TRUE_STATE_FOLDER  = strcat(SIMU_FOLDER , 'trueState/');
SIMU_SIM_DATA_FOLDER  = strcat(SIMU_FOLDER , 'simData/');
SIMU_RESULTS_DATA_FOLDER  = strcat(SIMU_FOLDER , 'results/');


simSpecFolder = 'singleProc_noStop/';
satnum = [1];
figdata = 1:4000;
for i = 1: length(satnum)
    sat = satnum(i);
    % data dump in 
    trueData = load(strcat(SIMU_TRUE_STATE_FOLDER,'sat',num2str(sat),'.mat'));
    simData = load(strcat(SIMU_SIM_DATA_FOLDER,simSpecFolder,'sat',num2str(sat),'.mat'));
    
    [~,ixTrue,ixSim]=intersect(trueData.timeTags,simData.timeTags);

    trueData.timeTags           =      trueData.timeTags (:,ixTrue)  ;     
    trueData.kineStateMeans     =trueData.kineStateMeans (:,ixTrue);
    trueData.kineStateCovs      =trueData.kineStateCovs (:,:,ixTrue);
    trueData.timeDiffMeans      =trueData.timeDiffMeans (:,ixTrue);
    trueData.timeDiffCovs       =trueData.timeDiffCovs   (:,ixTrue) ;  

    simData.timeTags           =simData.timeTags (:,ixSim)  ;     
    simData.kineStateMeans     =simData.kineStateMeans (:,ixSim);
    simData.kineStateCovs      =simData.kineStateCovs (:,:,ixSim);
    simData.timeDiffMeans      =simData.timeDiffMeans (:,ixSim);
    simData.timeDiffCovs       =simData.timeDiffCovs   (:,ixSim) ;  

    meanDiff = simData.kineStateMeans-trueData.kineStateMeans;
    timeTags=( simData.timeTags-simData.timeTags(1))/86400;
    
    figure();
    plot(timeTags(figdata),meanDiff(:,figdata)');
    title('state error');
    xlabel('day');
    legend x y z v_x v_y v_z
    
    figure()
    trs = sqrt(traceSeries(simData.kineStateCovs));
    plot(timeTags(figdata),trs(figdata));
    hold on
    covdiags = sqrt(diagSeries(simData.kineStateCovs));
    plot(timeTags(figdata),covdiags(:,figdata)');
    title('estimated covs');
    xlabel('day');
    legend rmse stdx stdy stdz stdv_x stdv_y stdv_z
end


simSpecFolder = 'parallel/';
satnum = [1];
figdata = 1:4000;
for i = 1: length(satnum)
    sat = satnum(i);
    % data dump in 
    trueData = load(strcat(SIMU_TRUE_STATE_FOLDER,'sat',num2str(sat),'.mat'));
    simData = load(strcat(SIMU_SIM_DATA_FOLDER,simSpecFolder,'sat',num2str(sat),'.mat'));
    
    [~,ixTrue,ixSim]=intersect(trueData.timeTags,simData.timeTags);

    trueData.timeTags           =      trueData.timeTags (:,ixTrue)  ;     
    trueData.kineStateMeans     =trueData.kineStateMeans (:,ixTrue);
    trueData.kineStateCovs      =trueData.kineStateCovs (:,:,ixTrue);
    trueData.timeDiffMeans      =trueData.timeDiffMeans (:,ixTrue);
    trueData.timeDiffCovs       =trueData.timeDiffCovs   (:,ixTrue) ;  

    simData.timeTags           =simData.timeTags (:,ixSim)  ;     
    simData.kineStateMeans     =simData.kineStateMeans (:,ixSim);
    simData.kineStateCovs      =simData.kineStateCovs (:,:,ixSim);
    simData.timeDiffMeans      =simData.timeDiffMeans (:,ixSim);
    simData.timeDiffCovs       =simData.timeDiffCovs   (:,ixSim) ;  

    meanDiff = simData.kineStateMeans-trueData.kineStateMeans;
    timeTags=( simData.timeTags-simData.timeTags(1))/86400;
    
    figure();
    plot(timeTags(figdata),meanDiff(:,figdata)');
    title('para state error');
    xlabel('day');
    legend x y z v_x v_y v_z
    
    figure()
    trs = sqrt(traceSeries(simData.kineStateCovs));
    plot(timeTags(figdata),trs(figdata));
    hold on
    covdiags = sqrt(diagSeries(simData.kineStateCovs));
    plot(timeTags(figdata),covdiags(:,figdata)');
    title('para estimated covs');
    xlabel('day');
    legend rmse stdx stdy stdz stdv_x stdv_y stdv_z
end
